#include <WisdomElement.h>
WisdomElement upcombot;
WisdomElement_Sensor ColorSensor1;

WisdomElement_Sensor Trail2;  
WisdomElement_Sensor Trail3;
WisdomElement_Sensor Trail4;

char x = 17;
float pid_value;
int ttt;
int speedbbb = 20;
struct StructPid 
{
  float kp;
  int ki;
  float kd;
  int inter;
  int err=0;
  int err_l;
  int ec;
  float value;
};
struct StructPid pid;

void setup() 
{
    Serial.begin(115200);
    ColorSensor1.Begin(1);  //颜色传感器开始
    Trail2.Begin(2);   // 巡线传感器
    Trail3.Begin(3);  //巡线传感器  
    Trail4.Begin(4); 
    
    upcombot.WaitBegin();
}
int cnt2 = 0;
int flag = 0;
int color_value1,color_value2,color_value3,pigu;
int speed_temp;
int flag2 = 0;
int cnt=0;
void loop()
{       
       
        pigu = ColorSensor1. COLOR_ReflexRead();
        color_value1=Trail2. COLOR_ReflexRead();// get left sensor vaule
        color_value2=Trail3. COLOR_ColorRead(); //get right sensor vaule
        color_value3=Trail4. COLOR_ColorRead();

        //转换巡线方式
        /*
        void moving()
        {
        switch(flag2)
        {
          case 0:moving0();break;
          
          case 1:moving1();break;
        }
        }
        */

        //摄像头巡线条件设定
        if(  color_value1<15 and color_value2 == 1 and color_value3 == 1 and cnt==0)
        {
          flag=1;
          cnt = 1;
        }
        if(uart1_datapackage.t_flag==2){ttt=2;}
        
        switch(flag)
        {
          case 0:moving();break;
          case 1:
                  if (ttt == 2)  //如果扫到码，跳到flag2
                  {
                    flag=2;
                    upcombot.MotionRun_name((uint8*)"激活武器"); 
             
                 upcombot.MoveControl(0,15,0,100); 
                 delay(100);
                 upcombot.MoveControl(0,0,-220,400);  
                 delay(400);
                 //speedbbb = 0;
                
                  }
                  /*
                  else  //否则停住不走
                  {
                       upcombot.MoveControl(0,10,0,5); 
                       delay(800);
                  }
                  */
                break;
          /*    
          case 2:
          //if(ttt == 2 ) //跳进flag2如果扫过码再激活武器.
          upcombot.MotionRun_name((uint8*)"激活武器"); 
             
                 upcombot.MoveControl(0,15,0,200); 
                 delay(200);
                 upcombot.MoveControl(0,0,-220,400);  
                 delay(400);
                 speedbbb = 25;
                 flag = 3;
                flag2 = 1;  //激活过武器转为传感器巡线
                 break;
          */
          
         case 2:
            int left;
  int right;
  int left1;
  int right1;
/**获取巡线传感器数值**/
  //left=ColorSensor1. COLOR_ReflexRead();// get left sensor vaule
  //right=Trail2. COLOR_ReflexRead(); //get right sensor vaule
  left = pigu;
  right = color_value1;
  //left1 = ColorSensor1. COLOR_ColorRead();
  //right1=Trail2. COLOR_ColorRead(); 
  /**巡线传感器判断程序**/
  if((left>x)&&(right>x))  
  {
    upcombot.MoveControl(0,15,0,10); 
  }
  else if((left<=x)&&(right<=x))
  {
    upcombot.MoveControl(0,15,0,10); 
  } 
  else if((left<x)&&(right>x))
  {
   upcombot.MoveControl(90,4,100,5);  
  }
  else if((left>x)&&(right<x))
  {
   upcombot.MoveControl(270,4,-100,5); 
  }  
  if((left ==100 or right ==100))
  {  
     
     upcombot.MoveControl(0,13,0,1000);
          delay(1000);  
          upcombot.MotionRun_name((uint8*)"击鼓"); 
                  delay(6000);          
                  upcombot.MotionRun_name((uint8*)"击鼓"); 
                  delay(6000);
                  upcombot.MotionRun_name((uint8*)"击鼓"); 
                  delay(6000);
                  
                  while(1){
                            upcombot.MoveControl(0,0,0,0); 
                  }
                  
     
     
  }
        
        
       
}
}
void moving()
{
  pid_value = uart1_datapackage.pid_h*256 + uart1_datapackage.pid_l;
  if(uart1_datapackage.direction == 0)
       upcombot.MoveControl(0 , speedbbb, pid_value, 5);
 else 
       upcombot.MoveControl(0, speedbbb, -pid_value, 5);
 }
 //pid
 /*
float outp_limit = 1000;
float outd_limit = 1000;
float out_limit = 1000;
 float PID_PositionCal(int set, int act)
        {
    float cal_p,cal_d,temp;
    //err更新 
    pid.kp = 4;  //p4 d12 速度20还可以
    pid.kd = 9;
    pid.err_l = pid.err;
    pid.err = set-act;
    pid.ec = pid.err - pid.err_l;
    cal_p = pid.kp * (float)pid.err ;
    cal_d = pid.kd * (float)pid.ec;
    
    //p限幅 
    if (cal_p > outp_limit) cal_p = outp_limit;
    if (cal_p < -outp_limit) cal_p = -outp_limit;
    
    if (cal_d > outd_limit) cal_d = outd_limit;
    if (cal_d < -outd_limit) cal_d = -outd_limit;
    
    temp = cal_p + cal_d;

    if (temp > out_limit) temp = out_limit;
    if (temp < -out_limit) temp = -out_limit;
    //pid
    pid.value = temp;
    return pid.value;
}
*/
void moving1()
{

  int left;
  int right;
  int left1;
  int right1;
/**获取巡线传感器数值**/
  //left=ColorSensor1. COLOR_ReflexRead();// get left sensor vaule
  //right=Trail2. COLOR_ReflexRead(); //get right sensor vaule
  left = pigu;
  right = color_value1;
  //left1 = ColorSensor1. COLOR_ColorRead();
  //right1=Trail2. COLOR_ColorRead(); 
  /**巡线传感器判断程序**/
  if((left>x)&&(right>x))  
  {
    upcombot.MoveControl(0,15,0,10); 
  }
  else if((left<=x)&&(right<=x))
  {
    upcombot.MoveControl(0,15,0,10); 
  } 
  else if((left<x)&&(right>x))
  {
   upcombot.MoveControl(90,4,100,5);  
  }
  else if((left>x)&&(right<x))
  {
   upcombot.MoveControl(270,4,-100,5); 
  }  
  /*
  else if(left1 == 5 or right == 5)
  {
    upcombot.MoveControl(0,10,0,5);
          delay(500);  
          upcombot.MotionRun_name((uint8*)"击鼓"); 
                  delay(6000);          
                  upcombot.MotionRun_name((uint8*)"击鼓"); 
                  delay(6000);
                  upcombot.MotionRun_name((uint8*)"击鼓"); 
                  delay(6000);
                  
                  while(1){
                            upcombot.MoveControl(0,0,0,0); 
                  }
                  */

  
  /*
    int left;
    int right;
    int left1;
    int right1;
    int value_minus;
    static int last_value=0;
    left = (int)ColorSensor1. COLOR_ReflexRead();// get left sensor vaule获得左传感器值
    right = (int)Trail2. COLOR_ReflexRead(); //get right sensor vaule获得右传感器值
    left1 = (int)ColorSensor1. COLOR_ColorRead();
    right1 = (int)Trail2. COLOR_ColorRead();
    value_minus = left - right;
      if((left>x)&&(right>x))  
    {
      PID_PositionCal(0,value_minus);
      upcombot.MoveControl(0,20,pid.value,8); // 直 走
    }
    else if((left<=x)&&(right<=x))
    {
      PID_PositionCal(0,value_minus);
      upcombot.MoveControl(0,20,pid.value,8 ); //直走
    } 
    else if((left<x)&&(right>x))
    {
      PID_PositionCal(0,value_minus);
        upcombot.MoveControl(0,4,pid.value,10);  // 左移
    }
    else if((left>x)&&(right<x))
    {
      PID_PositionCal(0,value_minus);
      upcombot.MoveControl(0,4,pid.value,10);  // 右移
    
    }
    else if (left1 == 5 or right1 == 5)
    {
      
          upcombot.MoveControl(0,10,0,5);
          delay(500);  
          upcombot.MotionRun_name((uint8*)"击鼓"); 
                  delay(6000);          
                  upcombot.MotionRun_name((uint8*)"击鼓"); 
                  delay(6000);
                  upcombot.MotionRun_name((uint8*)"击鼓"); 
                  delay(6000);
                  
                  while(1){
                            upcombot.MoveControl(0,0,0,0); 
                  }
                  */
    }
  

 
